#include "BvhJoint.h"

BvhJoint::BvhJoint()
	:QObject()
{
	m_parent = 0;
	m_pos << 0,0,0;
	m_rot << 0,0,0;
}

BvhJoint::~BvhJoint()
{
	m_parent = 0;
}

BvhJoint::BvhJoint(const BvhJoint& bj)
{
	m_name = bj.m_name;
	m_offset = bj.m_offset;
	m_channelList = bj.m_channelList;
	m_childList = bj.m_childList;
	m_parent = bj.m_parent;
}

BvhJoint& BvhJoint::operator=(const BvhJoint& bj)
{
	m_name = bj.m_name;
	m_offset = bj.m_offset;
	m_channelList = bj.m_channelList;
	m_childList = bj.m_childList;
	m_parent = bj.m_parent;
	return *this;
}

QString BvhJoint::getName()
{
	return m_name;
}

void BvhJoint::setName(QString jointName)
{
	m_name = jointName;
}

Vector3f BvhJoint::getOffset()
{
	return m_offset;
}

void BvhJoint::setOffset(Vector3f vec)
{
	m_offset = vec;
}

bool BvhJoint::containsChannel(QString chanName)
{
	if (m_channelList.contains(chanName)) return true;
	else return false;
}

QString BvhJoint::getChannel(int i)
{
	return m_channelList.value(i);
}

int BvhJoint::getChannelCount()
{
	return m_channelList.count();
}

int BvhJoint::getTotalChannelCount()
{
	int count = getChannelCount();
	for (int n=0;n<getChildCount();n++) {
		count += getChild(n)->getTotalChannelCount();
	}
	return count;
}

void BvhJoint::addChannel(QString chanName)
{
	m_channelList.append(chanName);
}

void BvhJoint::addPosChannels()
{
	m_channelList.append("Xposition");
	m_channelList.append("Yposition");
	m_channelList.append("Zposition");
}

void BvhJoint::addRotChannels()
{
	m_channelList.append("Xrotation");
	m_channelList.append("Yrotation");
	m_channelList.append("Zrotation");
}

QPointer<BvhJoint> BvhJoint::getChild(int i)
{
	return m_childList[i];
}

int BvhJoint::getChildCount()
{
	return m_childList.count();
}

void BvhJoint::addChild(BvhJoint* joint)
{
	m_childList.append(joint);
}

void BvhJoint::addEndSite()
{
	BvhJoint* joint = new BvhJoint();
	joint->setName("End Site");
	Vector3f zeroVec; zeroVec << 0, 0, 0;
	joint->setOffset(zeroVec);
	m_childList.append(joint);
}

QPointer<BvhJoint> BvhJoint::getParent()
{
	return m_parent;
}

void BvhJoint::setParent(BvhJoint* parentJoint)
{
	m_parent = parentJoint;
}

void BvhJoint::setFromData(QList<double> data)
{
	int index = 0;
	QList<BvhJoint*> jointList = getJointList();
	for (int n=0;n<jointList.count();n++) {
		for (int d=0;d<jointList[n]->getChannelCount();d++) {
			QString chanName = jointList[n]->getChannel(d);
			jointList[n]->setFromData(chanName, data.value(index));
			index++;
		}
	}
	//for (int d=0;d<getChannelCount();d++) {
	//	setFromData(getChannel(d),data.value(0));
	//	data.removeAt(0);
	//}
	//
	//for (int n=0;n<m_childList.count();n++) {
	//	getChild(n)->setFromData(data);
	//}
}

void BvhJoint::setFromData(QString channel, double data)
{
	if (channel == "Xposition") m_pos[0] = data;
	else if (channel == "Yposition") m_pos[1] = data;
	else if (channel == "Zposition") m_pos[2] = data;
	else if (channel == "Xrotation") m_rot[0] = data;
	else if (channel == "Yrotation") m_rot[1] = data;
	else if (channel == "Zrotation") m_rot[2] = data;

}

int BvhJoint::getJointCount()
{
	int count = 1;
	for (int n=0;n<getChildCount();n++) {
		count += getChild(n)->getJointCount();
	}
	return count;
}

QList<BvhJoint*> BvhJoint::getJointList()
{
	 QList<BvhJoint*> jointList;
	 jointList.append(this);
	 for (int n=0;n<getChildCount();n++) {
		 QList<BvhJoint*> childList = getChild(n)->getJointList();
		 jointList.append(childList);
	 }
	 return jointList;
}

Matrix3f BvhJoint::calcRotMatrixLocal()
{
	if (!getParent()) return Matrix3f().Identity();
	//if (!getParent()->getParent()) return Matrix3f().Identity();
	Vector3f rot = getParent()->getRot() * 3.14159/180.0;
	Matrix3f rotx, roty, rotz, rotzyx;
	rotx << 1, 0, 0, 0, cos(rot[0]), -sin(rot[0]), 0, sin(rot[0]), cos(rot[0]);
	roty << cos(rot[1]), 0, sin(rot[1]), 0, 1, 0, -sin(rot[1]), 0, cos(rot[1]);
	rotz << cos(rot[2]), -sin(rot[2]), 0, sin(rot[2]), cos(rot[2]), 0, 0, 0, 1;
	rotzyx = rotx * roty * rotz;
	return rotzyx;
}

Matrix3f BvhJoint::calcRotMatrixLocal(double x1, double x2, double x3)
{
	double df = 3.14159/180.0;
	Vector3f rot;
	rot << x1*df , x2*df , x3*df;
	Matrix3f rotx, roty, rotz, rotzyx;
	rotx << 1, 0, 0, 0, cos(rot[0]), -sin(rot[0]), 0, sin(rot[0]), cos(rot[0]);
	roty << cos(rot[1]), 0, sin(rot[1]), 0, 1, 0, -sin(rot[1]), 0, cos(rot[1]);
	rotz << cos(rot[2]), -sin(rot[2]), 0, sin(rot[2]), cos(rot[2]), 0, 0, 0, 1;
	rotzyx = rotx * roty * rotz;
	return rotzyx;
}

Matrix3f BvhJoint::calcRotMatrixGlobal()
{
	if (!getParent()) return Matrix3f().Identity();
	//if (!getParent()->getParent()) return Matrix3f().Identity();
	Matrix3f mat = Matrix3f::Identity();
	BvhJoint* jointPtr = getParent();
	while (jointPtr) {
		mat = jointPtr->calcRotMatrixLocal()*mat;
		jointPtr = jointPtr->getParent();
	}
	return mat;
}

Vector3f BvhJoint::calcPosGlobal()
{
	Matrix3f gMat = calcRotMatrixGlobal();
	Matrix3f lMat = calcRotMatrixLocal();
	Vector3f curPos = Vector3f::Zero();
	if (getParent()) curPos = getParent()->calcPosGlobal();
	Vector3f gPos = curPos + gMat * lMat * (m_offset + m_pos);
	return gPos;
}

Vector3f BvhJoint::calcGPos(Vector3f pos)
{
	Matrix3f lMat = calcRotMatrixLocal();
	Vector3f newPos = lMat*(m_offset + m_pos + pos);
	if (getParent()) return getParent()->calcGPos(newPos);
	else return newPos;
}

Vector3f BvhJoint::getPos()
{
	//Vector3f prevPos;
	//if (m_parent) prevPos = m_parent->getPos();
	//else prevPos << 0 , 0 , 0;
	//Vector3f currentPos = prevPos + m_offset + m_pos;
	//return currentPos;
	return m_pos;
}

Vector3f BvhJoint::getRot()
{
	return m_rot;
}

QMatrix4x4 BvhJoint::calcMatrix()
{
	QMatrix4x4 mat;
	mat.translate(m_offset.x(),m_offset.y(),m_offset.z());
	if (m_parent) mat = m_parent->calcMatrix(mat);
	return mat;
}

QMatrix4x4 BvhJoint::calcMatrix(QMatrix4x4 mat)
{
	// current assumes rotation order is xyz which is not always true for bvh
	if (m_parent) {
		mat.rotate(m_rot.x(),1,0,0);
		mat.rotate(m_rot.y(),0,1,0);
		mat.rotate(m_rot.z(),0,0,1);
	}
	mat.translate(m_offset.x(),m_offset.y(),m_offset.z());
	if (m_parent) return m_parent->calcMatrix(mat);
	else return mat;
}

//void BvhJoint::addTransformationsOne(QGLSceneNode* node)
//{
//	QList<BvhJoint*> jointList;
//	BvhJoint* curJoint = this;
//	while (curJoint->getParent()) {
//		curJoint = curJoint->getParent();
//		jointList.insert(0,curJoint);
//	}
//	for (int n=0;n<jointList.count();n++) {
//		jointList[n]->addTransformations(node,1);
//	}
//	addTransformations(node);
//}
//
//void BvhJoint::addTransformations(QGLSceneNode* node, int val)
//{
//	if (m_parent && val != 0) {
//		QGraphicsRotation3D* gt = new QGraphicsRotation3D();
//		gt->setAxis(QVector3D(1,0,0));
//		gt->setAngle(getRot()[0]);
//		node->addTransform(gt);
//		rotgt[0] = gt;
//		gt = new QGraphicsRotation3D();
//		gt->setAxis(QVector3D(0,1,0));
//		gt->setAngle(m_rot[1]);
//		node->addTransform(gt);
//		rotgt[1] = gt;
//		gt = new QGraphicsRotation3D();
//		gt->setAxis(QVector3D(0,0,1));
//		gt->setAngle(m_rot[2]);
//		node->addTransform(gt);
//		rotgt[2] = gt;
//	}
//	QGraphicsTranslation3D* gt = new QGraphicsTranslation3D();
//	QVector3D vec(m_offset.x(),m_offset.y(),m_offset.z());
//	gt->setTranslate(vec);
//	node->addTransform(gt);
//	trgt = gt;
//	//if (m_parent) m_parent->addTransformations(node,val+1);
//}
//
//void BvhJoint::updateTransformations()
//{
//	for (int n=0;n<3;n++) {
//		if (rotgt[n]) rotgt[n]->setAngle(m_rot[n]);
//	}
//	if (trgt) {
//		QVector3D vec(m_offset.x(),m_offset.y(),m_offset.z());
//		trgt->setTranslate(vec);
//	}
//	if (m_parent) m_parent->updateTransformations();
//}